//
// Created by ubuntu on 19-5-9.
//

#ifndef RELOCATION_SLAM_KDTREE_SEARCH_H
#define RELOCATION_SLAM_KDTREE_SEARCH_H

#include <opencv2/opencv.hpp>
#include <opencv2/flann.hpp>
using namespace std;
void KdtreeSearch(vector<cv::Point3f> m_pOriPtXYZ){
    /** 建立kd树索引 **/
/* features */
    cv::Mat source = cv::Mat(m_pOriPtXYZ).reshape(1); //vector<Point3f> m_pOriPtXY
//即m_pOriPtXY是Point3f类型的vector数组，由于建立索引需要mat类型，就直接用m_pOriPtXY去生成一个mat对象
    source.convertTo(source,CV_32F);
/* params */
    cv::flann::KDTreeIndexParams indexParams(2); //此处我将trees参数设置为2（这也就是kd-trees唯一需要设置的params）
    cv::flann::Index kdtree(source, indexParams); //kd树索引建立完毕


///**预设knnSearch所需参数及容器**/
//    unsigned queryNum = 5;//用于设置返回邻近点的个数
//    vector<float> vecQuery(3);//存放 查询点 的容器（本例都是vector类型）
//    vector<int> vecIndex(queryNum);//存放返回的点索引
//    vector<float> vecDist(queryNum);//存放距离
//    flann::SearchParams params(32);//设置knnSearch搜索参数
//
///**KD树knn查询**/
//    vecQuery[0] = (float)-0.5494; //查询点x坐标
//    vecQuery[1] = (float)1.9748; //查询点y坐标
//    vecQuery[2] = (float)1.5435; //查询点z坐标
//    kdtree.knnSearch(vecQuery, vecIndex, vecDist, queryNum, params);

//    for(int i=0;i<vecIndex.size();i++)
//        cout<<"vecIndex:"<<i<<"::"<<vecIndex[i];
//    for(int i=0;i<vecDist.size();i++)
//        cout<<"vecDist:"<<i<<"::"<<vecDist[i];

/**预设radiusSearch所需参数及容器**/
    float radius = 0.50;//用于设置搜索半径
    int maxResults = 50;
    vector<int> vecIndex;//存放返回的点索引
    vector<float> vecDist;//存放距离
    cv::flann::SearchParams params(32);//设置knnSearch搜索参数

/**KD树radiusSearch查询**/
    vector<float> vecQuery(3);//存放 查询点 的容器（本例都是vector类型）
    vecQuery[0] = (float)-0.5494; //查询点x坐标
    vecQuery[1] = (float)1.9748; //查询点y坐标
    vecQuery[2] = (float)1.5435; //查询点z坐标
    kdtree.radiusSearch(vecQuery,vecIndex,vecDist,radius,maxResults,params);

//    for(int i=0;i<vecIndex.size();i++)
//        cout<<"vecIndex:"<<i<<"::"<<vecIndex[i]<<endl;
//    for(int i=0;i<vecDist.size();i++)
//        cout<<"vecDist:"<<i<<"::"<<vecDist[i]<<endl;
}

#endif //RELOCATION_SLAM_KDTREE_SEARCH_H
